/*
 * File Name:               XCapsule3D.cs
 * 
 * Description:             胶囊体算法
 * Todo:                    在x1x2 或者其他值极为相近的时候会出现超值域的情况
 * Author:                  lisiyu <576603306@qq.com>

 * Create Date:             2017/06/20
 */

public struct XCapsule3D
{
    public XVector3 Point01;
    public XVector3 Point02;
    public XNumber Radius;

    public XCapsule3D(XVector3 pos01, XVector3 pos02, XNumber radius)
    {
        Point01 = pos01;
        Point02 = pos02;
        Radius = radius;
    }

    public struct CrossData
    {
        public XVector3 CrossPos;           // 交点
        public XNumber CrossLerp;           // 交点差值
        public XVector3 CrossNormal;        // 交点法线
    }

    private void handleLeft(XNumber xMin, XNumber a, XNumber r, XNumber xMax, XNumber yMin, XNumber yMax, XNumber zMin, XNumber zMax, XNumber x1, XNumber x2, XNumber y1, XNumber y2, XNumber z1, XNumber z2, XVector3 localSegPos01, ref XNumber dis, ref XVector3 localCrossPos)
    {
        if (XIntMath.Max(xMin, -a - r) <= XIntMath.Min(xMax, -a)
            && XIntMath.Max(yMin, -r) <= XIntMath.Min(yMax, r)
            && XIntMath.Max(zMin, -r) <= XIntMath.Min(zMax, r))
        {
            // 1
            if (x1 != x2 && y1 != y2 && z1 != z2)
            {
                var A = (y1 - y2) / (x1 - x2);
                var B = (x1 * y2 - x2 * y1) / (x1 - x2);
                var C = (z1 - z2) / (x1 - x2);
                var D = (x1 * z2 - x2 * z1) / (x1 - x2);

                var A1_1 = A * A + C * C + 1;
                var B1_1 = 2 * (A * B + C * D + a);
                var C1_1 = B * B + D * D + a * a - r * r;

                var S1_1 = B1_1 * B1_1 - 4 * A1_1 * C1_1;
                if (S1_1 >= 0)
                {
                    var X1_1_1 = (-B1_1 + XIntMath.Sqrt(S1_1)) / (2 * A1_1);
                    if (X1_1_1 >= XIntMath.Max(xMin, -a - r) && X1_1_1 <= XIntMath.Min(xMax, a))
                    {
                        var y = A * X1_1_1 + B;
                        var z = C * X1_1_1 + D;

                        var tmpPos = new XVector3(X1_1_1, y, z);
                        var tmpDis = XVector3.Distance(tmpPos, localSegPos01);
                        if (tmpDis < dis)
                        {
                            dis = tmpDis;
                            localCrossPos = tmpPos;
                        }
                    }

                    var X1_1_2 = (-B1_1 - XIntMath.Sqrt(S1_1)) / (2 * A1_1);
                    if (X1_1_2 >= XIntMath.Max(xMin, -a - r) && X1_1_2 <= XIntMath.Min(xMax, a))
                    {
                        var y = A * X1_1_2 + B;
                        var z = C * X1_1_2 + D;

                        var tmpPos = new XVector3(X1_1_2, y, z);
                        var tmpDis = XVector3.Distance(tmpPos, localSegPos01);
                        if (tmpDis < dis)
                        {
                            dis = tmpDis;
                            localCrossPos = tmpPos;
                        }
                    }
                }
            }
            // 2
            else if (x1 == x2 && y1 != y2 && z1 != z2)
            {
                var E = (z1 - z2) / (y1 - y2);
                var F = (y2 * z1 + y1 * z2) / (y1 - y2);

                var A1_2 = E * E + 1;
                var B1_2 = 2 * F;
                var C1_2 = F * F + (x1 + a) * (x1 + a) - r * r;

                var S1_2 = B1_2 * B1_2 - 4 * A1_2 * C1_2;
                if (S1_2 >= 0)
                {
                    var Y1_2_1 = (-B1_2 + XIntMath.Sqrt(S1_2)) / (2 * A1_2);
                    if (Y1_2_1 >= yMin && Y1_2_1 <= yMax)
                    {
                        var x = x1;
                        var z = E * Y1_2_1 + F;

                        var tmpPos = new XVector3(x, Y1_2_1, z);
                        var tmpDis = XVector3.Distance(tmpPos, localSegPos01);
                        if (tmpDis < dis)
                        {
                            dis = tmpDis;
                            localCrossPos = tmpPos;
                        }
                    }

                    var Y1_2_2 = (-B1_2 - XIntMath.Sqrt(S1_2)) / (2 * A1_2);
                    if (Y1_2_2 >= yMin && Y1_2_2 <= yMax)
                    {
                        var x = x1;
                        var z = E * Y1_2_2 + F;

                        var tmpPos = new XVector3(x, Y1_2_2, z);
                        var tmpDis = XVector3.Distance(tmpPos, localSegPos01);
                        if (tmpDis < dis)
                        {
                            dis = tmpDis;
                            localCrossPos = tmpPos;
                        }
                    }
                }
            }
            // 3
            else if (x1 == x2 && y1 == y2 && z1 != z2)
            {
                var C1_3 = (x1 + a) * (x1 + a) + y1 * y1 - r * r;
                var S1_3 = -4 * C1_3;
                if (S1_3 >= 0)
                {
                    var Z1_3_1 = XIntMath.Sqrt(S1_3) / 2;
                    if (Z1_3_1 >= zMin && Z1_3_1 <= zMax)
                    {
                        var x = x1;
                        var y = y1;

                        var tmpPos = new XVector3(x, y, Z1_3_1);
                        var tmpDis = XVector3.Distance(tmpPos, localSegPos01);
                        if (tmpDis < dis)
                        {
                            dis = tmpDis;
                            localCrossPos = tmpPos;
                        }
                    }

                    var Z1_3_2 = -Z1_3_1;
                    if (Z1_3_2 >= zMin && Z1_3_2 <= zMax)
                    {
                        var x = x1;
                        var y = y1;

                        var tmpPos = new XVector3(x, y, Z1_3_2);
                        var tmpDis = XVector3.Distance(tmpPos, localSegPos01);
                        if (tmpDis < dis)
                        {
                            dis = tmpDis;
                            localCrossPos = tmpPos;
                        }
                    }
                }
            }
            // 4
            else if (x1 == x2 && y1 != y2 && z1 == z2)
            {
                var C1_4 = (x1 + a) * (x1 + a) + z1 * z1 - r * r;
                var S1_4 = -4 * C1_4;
                if (S1_4 >= 0)
                {
                    var Y1_4_1 = XIntMath.Sqrt(S1_4) / 2;
                    if (Y1_4_1 >= yMin && Y1_4_1 <= yMax)
                    {
                        var x = x1;
                        var z = z1;

                        var tmpPos = new XVector3(x, Y1_4_1, z);
                        var tmpDis = XVector3.Distance(tmpPos, localSegPos01);
                        if (tmpDis < dis)
                        {
                            dis = tmpDis;
                            localCrossPos = tmpPos;
                        }
                    }

                    var Y1_4_2 = -Y1_4_1;
                    if (Y1_4_2 >= yMin && Y1_4_2 <= yMax)
                    {
                        var x = x1;
                        var z = z1;

                        var tmpPos = new XVector3(x, Y1_4_2, z);
                        var tmpDis = XVector3.Distance(tmpPos, localSegPos01);
                        if (tmpDis < dis)
                        {
                            dis = tmpDis;
                            localCrossPos = tmpPos;
                        }
                    }
                }
            }
            // 5
            else if (x1 != x2 && y1 == y2 && z1 != z2)
            {
                var C = (z1 - z2) / (x1 - x2);
                var D = (x1 * z2 - x2 * z1) / (x1 - x2);
                // z = C * x + D

                var A1_5 = C * C + 1;
                var B1_5 = 2 * (C * D + a);
                var C1_5 = y1 * y1 + D * D + a * a - r * r;

                var S1_5 = B1_5 * B1_5 - 4 * A1_5 * C1_5;
                if (S1_5 >= 0)
                {
                    var X1_5_1 = (-B1_5 + XIntMath.Sqrt(S1_5)) / (2 * A1_5);
                    if (X1_5_1 >= XIntMath.Max(xMin, -a - r) && X1_5_1 <= XIntMath.Min(xMax, a))
                    {
                        var y = y1;
                        var z = C * X1_5_1 + D;

                        var tmpPos = new XVector3(X1_5_1, y, z);
                        var tmpDis = XVector3.Distance(tmpPos, localSegPos01);
                        if (tmpDis < dis)
                        {
                            dis = tmpDis;
                            localCrossPos = tmpPos;
                        }
                    }

                    var X1_5_2 = (-B1_5 - XIntMath.Sqrt(S1_5)) / (2 * A1_5);
                    if (X1_5_2 >= XIntMath.Max(xMin, -a - r) && X1_5_2 <= XIntMath.Min(xMax, a))
                    {
                        var y = y1;
                        var z = C * X1_5_2 + D;

                        var tmpPos = new XVector3(X1_5_2, y, z);
                        var tmpDis = XVector3.Distance(tmpPos, localSegPos01);
                        if (tmpDis < dis)
                        {
                            dis = tmpDis;
                            localCrossPos = tmpPos;
                        }
                    }
                }
            }
            // 6
            else if (x1 != x2 && y1 == y2 && z1 == z2)
            {
                var B1_6 = 2 * a;
                var C1_6 = y1 * y1 + z1 * z1 + a * a - r * r;
                var S1_6 = B1_6 * B1_6 - 4 * C1_6;
                if (S1_6 >= 0)
                {
                    var X1_6_1 = (-B1_6 + XIntMath.Sqrt(S1_6)) / 2;
                    if (X1_6_1 >= XIntMath.Max(xMin, -a - r) && X1_6_1 <= XIntMath.Min(xMax, a))
                    {
                        var y = y1;
                        var z = z1;

                        var tmpPos = new XVector3(X1_6_1, y, z);
                        var tmpDis = XVector3.Distance(tmpPos, localSegPos01);
                        if (tmpDis < dis)
                        {
                            dis = tmpDis;
                            localCrossPos = tmpPos;
                        }
                    }

                    var X1_6_2 = (-B1_6 - XIntMath.Sqrt(S1_6)) / 2;
                    if (X1_6_2 >= XIntMath.Max(xMin, -a - r) && X1_6_2 <= XIntMath.Min(xMax, a))
                    {
                        var y = y1;
                        var z = z1;

                        var tmpPos = new XVector3(X1_6_2, y, z);
                        var tmpDis = XVector3.Distance(tmpPos, localSegPos01);
                        if (tmpDis < dis)
                        {
                            dis = tmpDis;
                            localCrossPos = tmpPos;
                        }
                    }
                }
            }
            // 7
            else if (x1 != x2 && y1 != y2 && z1 == z2)
            {
                var A = (y1 - y2) / (x1 - x2);
                var B = (x1 * y2 - x2 * y1) / (x1 - x2);

                var A1_7 = A * A + 1;
                var B1_7 = 2 * (A * B + a);
                var C1_7 = B * B + z1 * z1 + a * a - r * r;
                var S1_7 = B1_7 * B1_7 - 4 * A1_7 * C1_7;
                if (S1_7 >= 0)
                {
                    var X1_7_1 = (-B1_7 + XIntMath.Sqrt(S1_7)) / (2 * A1_7);
                    if (X1_7_1 >= XIntMath.Max(xMin, -a - r) && X1_7_1 <= XIntMath.Min(xMax, a))
                    {
                        var y = A * X1_7_1 + B;
                        var z = z1;

                        var tmpPos = new XVector3(X1_7_1, y, z);
                        var tmpDis = XVector3.Distance(tmpPos, localSegPos01);
                        if (tmpDis < dis)
                        {
                            dis = tmpDis;
                            localCrossPos = tmpPos;
                        }
                    }

                    var X1_7_2 = (-B1_7 - XIntMath.Sqrt(S1_7)) / (2 * A1_7);
                    if (X1_7_2 >= XIntMath.Max(xMin, -a - r) && X1_7_2 <= XIntMath.Min(xMax, a))
                    {
                        var y = A * X1_7_2 + B;
                        var z = z1;

                        var tmpPos = new XVector3(X1_7_2, y, z);
                        var tmpDis = XVector3.Distance(tmpPos, localSegPos01);
                        if (tmpDis < dis)
                        {
                            dis = tmpDis;
                            localCrossPos = tmpPos;
                        }
                    }
                }
            }
            else
            {
                //Debug.LogError("未考虑到的取值范围 请检查");
            }
        }
    }

    private void handleMid(XNumber xMin, XNumber a, XNumber xMax, XNumber yMin, XNumber r, XNumber yMax, XNumber zMin, XNumber zMax, XNumber x1, XNumber x2, XNumber y1, XNumber y2, XNumber z1, XNumber z2, XVector3 localSegPos01, ref XNumber dis, ref XVector3 localCrossPos)
    {
        if (XIntMath.Max(xMin, -a) <= XIntMath.Min(xMax, a)
            && XIntMath.Max(yMin, -r) <= XIntMath.Min(yMax, r)
            && XIntMath.Min(zMin, -r) <= XIntMath.Min(zMax, r))
        {
            // 1
            if (x1 != x2 && y1 != y2 && z1 != z2)
            {
                var A = (y1 - y2) / (x1 - x2);
                var B = (x1 * y2 - x2 * y1) / (x1 - x2);
                var C = (z1 - z2) / (x1 - x2);
                var D = (x1 * z2 - x2 * z1) / (x1 - x2);

                var A2_1 = A * A + C * C;
                var B2_1 = 2 * (A * B + C * D);
                var C2_1 = B * B + D * D - r * r;
                var S2_1 = B2_1 * B2_1 - 4 * A2_1 * C2_1;
                if (S2_1 >= 0)
                {
                    var X2_1_1 = (-B2_1 + XIntMath.Sqrt(S2_1)) / (2 * A2_1);
                    if (X2_1_1 >= XIntMath.Max(xMin, -a) && X2_1_1 <= XIntMath.Min(xMax, a))
                    {
                        var y = A * X2_1_1 + B;
                        var z = C * X2_1_1 + D;

                        var tmpPos = new XVector3(X2_1_1, y, z);
                        var tmpDis = XVector3.Distance(tmpPos, localSegPos01);
                        if (tmpDis < dis)
                        {
                            dis = tmpDis;
                            localCrossPos = tmpPos;
                        }
                    }

                    var X2_1_2 = (-B2_1 - XIntMath.Sqrt(S2_1)) / (2 * A2_1);
                    if (X2_1_2 >= XIntMath.Max(xMin, -a) && X2_1_2 <= XIntMath.Min(xMax, a))
                    {
                        var y = A * X2_1_2 + B;
                        var z = C * X2_1_2 + D;

                        var tmpPos = new XVector3(X2_1_2, y, z);
                        var tmpDis = XVector3.Distance(tmpPos, localSegPos01);
                        if (tmpDis < dis)
                        {
                            dis = tmpDis;
                            localCrossPos = tmpPos;
                        }
                    }
                }
            }
            // 2
            else if (x1 == x2 && y1 != y2 && z1 != z2)
            {
                var E = (z1 - z2) / (y1 - y2);
                var F = (y2 * z1 + y1 * z2) / (y1 - y2);

                var A2_2 = E * E + 1;
                var B2_2 = 2 * F;
                var C2_2 = F * F - r * r;
                var S2_2 = B2_2 * B2_2 - 4 * A2_2 * C2_2;
                if (S2_2 >= 0)
                {
                    var Y2_2_1 = (-B2_2 + XIntMath.Sqrt(S2_2)) / (2 * A2_2);
                    if (Y2_2_1 >= yMin && Y2_2_1 <= yMax)
                    {
                        var x = x1;
                        var z = E * Y2_2_1 + F;

                        var tmpPos = new XVector3(x, Y2_2_1, z);
                        var tmpDis = XVector3.Distance(tmpPos, localSegPos01);
                        if (tmpDis < dis)
                        {
                            dis = tmpDis;
                            localCrossPos = tmpPos;
                        }
                    }

                    var Y2_2_2 = (-B2_2 - XIntMath.Sqrt(S2_2)) / (2 * A2_2);
                    if (Y2_2_2 >= yMin && Y2_2_2 <= yMax)
                    {
                        var x = x1;
                        var z = E * Y2_2_2 + F;

                        var tmpPos = new XVector3(x, Y2_2_2, z);
                        var tmpDis = XVector3.Distance(tmpPos, localSegPos01);
                        if (tmpDis < dis)
                        {
                            dis = tmpDis;
                            localCrossPos = tmpPos;
                        }
                    }
                }
            }
            // 3
            else if (x1 == x2 && y1 == y2 && z1 != z2)
            {
                var C2_3 = y1 * y1 - r * r;
                var S2_3 = -4 * C2_3;
                if (S2_3 >= 0)
                {
                    var Z2_3_1 = XIntMath.Sqrt(S2_3) / 2;
                    if (Z2_3_1 >= zMin && Z2_3_1 <= zMax)
                    {
                        var x = x1;
                        var y = y1;

                        var tmpPos = new XVector3(x, y, Z2_3_1);
                        var tmpDis = XVector3.Distance(tmpPos, localSegPos01);
                        if (tmpDis < dis)
                        {
                            dis = tmpDis;
                            localCrossPos = tmpPos;
                        }
                    }

                    var Z2_3_2 = -Z2_3_1;
                    if (Z2_3_2 >= zMin && Z2_3_2 <= zMax)
                    {
                        var x = x1;
                        var y = y1;

                        var tmpPos = new XVector3(x, y, Z2_3_2);
                        var tmpDis = XVector3.Distance(tmpPos, localSegPos01);
                        if (tmpDis < dis)
                        {
                            dis = tmpDis;
                            localCrossPos = tmpPos;
                        }
                    }
                }
            }
            // 4
            else if (x1 == x2 && y1 != y2 && z1 == z2)
            {
                var C2_4 = z1 * z1 - r * r;
                var S2_4 = -4 * C2_4;
                if (S2_4 >= 0)
                {
                    var Y2_4_1 = XIntMath.Sqrt(S2_4) / 2;
                    if (Y2_4_1 >= yMin && Y2_4_1 <= yMax)
                    {
                        var x = x1;
                        var z = z1;

                        var tmpPos = new XVector3(x, Y2_4_1, z);
                        var tmpDis = XVector3.Distance(tmpPos, localSegPos01);
                        if (tmpDis < dis)
                        {
                            dis = tmpDis;
                            localCrossPos = tmpPos;
                        }
                    }

                    var Y2_4_2 = -Y2_4_1;
                    if (Y2_4_2 >= yMin && Y2_4_2 <= yMax)
                    {
                        var x = x1;
                        var z = z1;

                        var tmpPos = new XVector3(x, Y2_4_2, z);
                        var tmpDis = XVector3.Distance(tmpPos, localSegPos01);
                        if (tmpDis < dis)
                        {
                            dis = tmpDis;
                            localCrossPos = tmpPos;
                        }
                    }
                }
            }
            // 5
            else if (x1 != x2 && y1 == y2 && z1 != z2)
            {
                var C = (z1 - z2) / (x1 - x2);
                var D = (x1 * z2 - x2 * z1) / (x1 - x2);
                // z = C * x + D

                var A2_5 = C * C;
                var B2_5 = 2 * C * D;
                var C2_5 = y1 * y1 + D * D - r * r;
                var S2_5 = B2_5 * B2_5 - 4 * A2_5 * C2_5;
                if (S2_5 >= 0)
                {
                    var X2_5_1 = (-B2_5 + XIntMath.Sqrt(S2_5)) / (2 * A2_5);
                    if (X2_5_1 >= XIntMath.Max(xMin, -a) && X2_5_1 <= XIntMath.Min(xMax, a))
                    {
                        var y = y1;
                        var z = C * X2_5_1 + D;

                        var tmpPos = new XVector3(X2_5_1, y, z);
                        var tmpDis = XVector3.Distance(tmpPos, localSegPos01);
                        if (tmpDis < dis)
                        {
                            dis = tmpDis;
                            localCrossPos = tmpPos;
                        }
                    }

                    var X2_5_2 = (-B2_5 - XIntMath.Sqrt(S2_5)) / (2 * A2_5);
                    if (X2_5_2 >= XIntMath.Max(xMin, -a) && X2_5_2 <= XIntMath.Min(xMax, a))
                    {
                        var y = y1;
                        var z = C * X2_5_2 + D;

                        var tmpPos = new XVector3(X2_5_2, y, z);
                        var tmpDis = XVector3.Distance(tmpPos, localSegPos01);
                        if (tmpDis < dis)
                        {
                            dis = tmpDis;
                            localCrossPos = tmpPos;
                        }
                    }
                }
            }
            // 6
            else if (x1 != x2 && y1 == y2 && z1 == z2)
            {
                // todo 擦边处理 目前认为不发生碰撞
            }
            // 7
            else if (x1 != x2 && y1 != y2 && z1 == z2)
            {
                var A = (y1 - y2) / (x1 - x2);
                var B = (x1 * y2 - x2 * y1) / (x1 - x2);

                var A2_7 = A * A;
                var B2_7 = 2 * A * B;
                var C2_7 = B * B + z1 * z1 - r * r;
                var S2_7 = B2_7 * B2_7 - 4 * A2_7 * C2_7;
                if (S2_7 >= 0)
                {
                    var X2_7_1 = (-B2_7 + XIntMath.Sqrt(S2_7)) / (2 * A2_7);
                    if (X2_7_1 >= XIntMath.Max(xMin, -a) && X2_7_1 <= XIntMath.Min(xMax, a))
                    {
                        var y = A * X2_7_1 + B;
                        var z = z1;

                        var tmpPos = new XVector3(X2_7_1, y, z);
                        var tmpDis = XVector3.Distance(tmpPos, localSegPos01);
                        if (tmpDis < dis)
                        {
                            dis = tmpDis;
                            localCrossPos = tmpPos;
                        }
                    }

                    var X2_7_2 = (-B2_7 - XIntMath.Sqrt(S2_7)) / (2 * A2_7);
                    if (X2_7_2 >= XIntMath.Max(xMin, -a) && X2_7_2 <= XIntMath.Min(xMax, a))
                    {
                        var y = A * X2_7_2 + B;
                        var z = z1;

                        var tmpPos = new XVector3(X2_7_2, y, z);
                        var tmpDis = XVector3.Distance(tmpPos, localSegPos01);
                        if (tmpDis < dis)
                        {
                            dis = tmpDis;
                            localCrossPos = tmpPos;
                        }
                    }
                }
            }
            else
            {
                //Debug.LogError("未考虑到的取值范围 请检查");
            }
        }
    }

    private void handleRight(XNumber xMin, XNumber a, XNumber xMax, XNumber r, XNumber yMin, XNumber yMax, XNumber zMin, XNumber zMax, XNumber x1, XNumber x2, XNumber y1, XNumber y2, XNumber z1, XNumber z2, XVector3 localSegPos01, ref XNumber dis, ref XVector3 localCrossPos)
    {
        if (XIntMath.Max(xMin, a) <= XIntMath.Min(xMax, a + r)
            && XIntMath.Max(yMin, -r) <= XIntMath.Min(yMax, r)
            && XIntMath.Min(zMin, -r) <= XIntMath.Min(zMax, r))
        {
            // 1
            if (x1 != x2 && y1 != y2 && z1 != z2)
            {
                var A = (y1 - y2) / (x1 - x2);
                var B = (x1 * y2 - x2 * y1) / (x1 - x2);
                var C = (z1 - z2) / (x1 - x2);
                var D = (x1 * z2 - x2 * z1) / (x1 - x2);

                var A3_1 = A * A + C * C + 1;
                var B3_1 = 2 * (A * B + C * D - a);
                var C3_1 = B * B + D * D + a * a - r * r;
                var S3_1 = B3_1 * B3_1 - 4 * A3_1 * C3_1;
                if (S3_1 >= 0)
                {
                    var X3_1_1 = (-B3_1 + XIntMath.Sqrt(S3_1)) / (2 * A3_1);
                    if (X3_1_1 >= XIntMath.Max(xMin, a) && X3_1_1 <= XIntMath.Min(xMax, a + r))
                    {
                        var y = A * X3_1_1 + B;
                        var z = C * X3_1_1 + D;

                        var tmpPos = new XVector3(X3_1_1, y, z);
                        var tmpDis = XVector3.Distance(tmpPos, localSegPos01);
                        if (tmpDis < dis)
                        {
                            dis = tmpDis;
                            localCrossPos = tmpPos;
                        }
                    }

                    var X3_1_2 = (-B3_1 - XIntMath.Sqrt(S3_1)) / (2 * A3_1);
                    if (X3_1_2 >= XIntMath.Max(xMin, a) && X3_1_2 <= XIntMath.Min(xMax, a + r))
                    {
                        var y = A * X3_1_2 + B;
                        var z = C * X3_1_2 + D;

                        var tmpPos = new XVector3(X3_1_2, y, z);
                        var tmpDis = XVector3.Distance(tmpPos, localSegPos01);
                        if (tmpDis < dis)
                        {
                            dis = tmpDis;
                            localCrossPos = tmpPos;
                        }
                    }
                }
            }
            // 2
            else if (x1 == x2 && y1 != y2 && z1 != z2)
            {
                var E = (z1 - z2) / (y1 - y2);
                var F = (y2 * z1 + y1 * z2) / (y1 - y2);

                var A3_2 = E * E + 1;
                var B3_2 = 2 * F;
                var C3_2 = F * F + (x1 - a) * (x1 - a) - r * r;

                var S3_2 = B3_2 * B3_2 - 4 * A3_2 * C3_2;
                if (S3_2 >= 0)
                {
                    var Y3_2_1 = (-B3_2 + XIntMath.Sqrt(S3_2)) / (2 * A3_2);
                    if (Y3_2_1 >= yMin && Y3_2_1 <= yMax)
                    {
                        var x = x1;
                        var z = E * Y3_2_1 + F;

                        var tmpPos = new XVector3(x, Y3_2_1, z);
                        var tmpDis = XVector3.Distance(tmpPos, localSegPos01);
                        if (tmpDis < dis)
                        {
                            dis = tmpDis;
                            localCrossPos = tmpPos;
                        }
                    }

                    var Y3_2_2 = (-B3_2 - XIntMath.Sqrt(S3_2)) / (2 * A3_2);
                    if (Y3_2_2 >= yMin && Y3_2_2 <= yMax)
                    {
                        var x = x1;
                        var z = E * Y3_2_2 + F;

                        var tmpPos = new XVector3(x, Y3_2_2, z);
                        var tmpDis = XVector3.Distance(tmpPos, localSegPos01);
                        if (tmpDis < dis)
                        {
                            dis = tmpDis;
                            localCrossPos = tmpPos;
                        }
                    }
                }
            }
            // 3
            else if (x1 == x2 && y1 == y2 && z1 != z2)
            {
                var C3_3 = (x1 - a) * (x1 - a) + y1 * y1 - r * r;
                var S3_3 = -4 * C3_3;
                if (S3_3 >= 0)
                {
                    var Z3_3_1 = XIntMath.Sqrt(S3_3) / 2;
                    if (Z3_3_1 >= zMin && Z3_3_1 <= zMax)
                    {
                        var x = x1;
                        var y = y1;

                        var tmpPos = new XVector3(x, y, Z3_3_1);
                        var tmpDis = XVector3.Distance(tmpPos, localSegPos01);
                        if (tmpDis < dis)
                        {
                            dis = tmpDis;
                            localCrossPos = tmpPos;
                        }
                    }

                    var Z3_3_2 = -Z3_3_1;
                    if (Z3_3_2 >= zMin && Z3_3_2 <= zMax)
                    {
                        var x = x1;
                        var y = y1;

                        var tmpPos = new XVector3(x, y, Z3_3_2);
                        var tmpDis = XVector3.Distance(tmpPos, localSegPos01);
                        if (tmpDis < dis)
                        {
                            dis = tmpDis;
                            localCrossPos = tmpPos;
                        }
                    }
                }
            }
            // 4
            else if (x1 == x2 && y1 != y2 && z1 == z2)
            {
                var C3_4 = (x1 - a) * (x1 - a) + z1 * z1 - r * r;
                var S3_4 = -4 * C3_4;
                if (S3_4 >= 0)
                {
                    var Y3_4_1 = XIntMath.Sqrt(S3_4) / 2;
                    if (Y3_4_1 >= yMin && Y3_4_1 <= yMax)
                    {
                        var x = x1;
                        var z = z1;

                        var tmpPos = new XVector3(x, Y3_4_1, z);
                        var tmpDis = XVector3.Distance(tmpPos, localSegPos01);
                        if (tmpDis < dis)
                        {
                            dis = tmpDis;
                            localCrossPos = tmpPos;
                        }
                    }

                    var Y3_4_2 = -Y3_4_1;
                    if (Y3_4_2 >= yMin && Y3_4_2 <= yMax)
                    {
                        var x = x1;
                        var z = z1;

                        var tmpPos = new XVector3(x, Y3_4_2, z);
                        var tmpDis = XVector3.Distance(tmpPos, localSegPos01);
                        if (tmpDis < dis)
                        {
                            dis = tmpDis;
                            localCrossPos = tmpPos;
                        }
                    }
                }
            }
            // 5
            else if (x1 != x2 && y1 == y2 && z1 != z2)
            {
                var C = (z1 - z2) / (x1 - x2);
                var D = (x1 * z2 - x2 * z1) / (x1 - x2);
                // z = C * x + D

                var A3_5 = C * C + 1;
                var B3_5 = 2 * (C * D - a);
                var C3_5 = y1 * y1 + D * D + a * a - r * r;
                var S3_5 = B3_5 * B3_5 - 4 * A3_5 * C3_5;
                if (S3_5 >= 0)
                {
                    var X3_5_1 = (-B3_5 + XIntMath.Sqrt(S3_5)) / (2 * A3_5);
                    if (X3_5_1 >= XIntMath.Max(xMin, a) && X3_5_1 <= XIntMath.Min(xMax, a + r))
                    {
                        var y = y1;
                        var z = C * X3_5_1 + D;

                        var tmpPos = new XVector3(X3_5_1, y, z);
                        var tmpDis = XVector3.Distance(tmpPos, localSegPos01);
                        if (tmpDis < dis)
                        {
                            dis = tmpDis;
                            localCrossPos = tmpPos;
                        }
                    }

                    var X3_5_2 = (-B3_5 - XIntMath.Sqrt(S3_5)) / (2 * A3_5);
                    if (X3_5_2 >= XIntMath.Max(xMin, a) && X3_5_2 <= XIntMath.Min(xMax, a + r))
                    {
                        var y = y1;
                        var z = C * X3_5_2 + D;

                        var tmpPos = new XVector3(X3_5_2, y, z);
                        var tmpDis = XVector3.Distance(tmpPos, localSegPos01);
                        if (tmpDis < dis)
                        {
                            dis = tmpDis;
                            localCrossPos = tmpPos;
                        }
                    }
                }
            }
            // 6
            else if (x1 != x2 && y1 == y2 && z1 == z2)
            {
                var B3_6 = -2 * a;
                var C3_6 = y1 * y1 + z1 * z1 + a * a - r * r;
                var S3_6 = B3_6 * B3_6 - 4 * C3_6;
                if (S3_6 >= 0)
                {
                    var X3_6_1 = (-B3_6 + XIntMath.Sqrt(S3_6)) / 2;
                    if (X3_6_1 >= XIntMath.Max(xMin, a) && X3_6_1 <= XIntMath.Min(xMax, a + r))
                    {
                        var y = y1;
                        var z = z1;

                        var tmpPos = new XVector3(X3_6_1, y, z);
                        var tmpDis = XVector3.Distance(tmpPos, localSegPos01);
                        if (tmpDis < dis)
                        {
                            dis = tmpDis;
                            localCrossPos = tmpPos;
                        }
                    }

                    var X3_6_2 = (-B3_6 - XIntMath.Sqrt(S3_6)) / 2;
                    if (X3_6_2 >= XIntMath.Max(xMin, a) && X3_6_2 <= XIntMath.Min(xMax, a + r))
                    {
                        var y = y1;
                        var z = z1;

                        var tmpPos = new XVector3(X3_6_2, y, z);
                        var tmpDis = XVector3.Distance(tmpPos, localSegPos01);
                        if (tmpDis < dis)
                        {
                            dis = tmpDis;
                            localCrossPos = tmpPos;
                        }
                    }
                }
            }
            // 7
            else if (x1 != x2 && y1 != y2 && z1 == z2)
            {
                var A = (y1 - y2) / (x1 - x2);
                var B = (x1 * y2 - x2 * y1) / (x1 - x2);

                var A3_7 = A * A + 1;
                var B3_7 = 2 * (A * B - a);
                var C3_7 = B * B + z1 * z1 + a * a - r * r;
                var S3_7 = B3_7 * B3_7 - 4 * A3_7 * C3_7;
                if (S3_7 >= 0)
                {
                    var X3_7_1 = (-B3_7 + XIntMath.Sqrt(S3_7)) / (2 * A3_7);
                    if (X3_7_1 >= XIntMath.Max(xMin, a) && X3_7_1 <= XIntMath.Min(xMax, a + r))
                    {
                        var y = A * X3_7_1 + B;
                        var z = z1;

                        var tmpPos = new XVector3(X3_7_1, y, z);
                        var tmpDis = XVector3.Distance(tmpPos, localSegPos01);
                        if (tmpDis < dis)
                        {
                            dis = tmpDis;
                            localCrossPos = tmpPos;
                        }
                    }

                    var X3_7_2 = (-B3_7 - XIntMath.Sqrt(S3_7)) / (2 * A3_7);
                    if (X3_7_2 >= XIntMath.Max(xMin, a) && X3_7_2 <= XIntMath.Min(xMax, a + r))
                    {
                        var y = A * X3_7_2 + B;
                        var z = z1;

                        var tmpPos = new XVector3(X3_7_2, y, z);
                        var tmpDis = XVector3.Distance(tmpPos, localSegPos01);
                        if (tmpDis < dis)
                        {
                            dis = tmpDis;
                            localCrossPos = tmpPos;
                        }
                    }
                }
            }
            else
            {
                //Debug.LogError("未考虑到的取值范围 请检查");
            }
        }
    }

    public bool cross(XVector3 segmentPos01, XVector3 segmentPos02, out CrossData crossData)
    {
        if (segmentPos01 == segmentPos02)
        {
            crossData = default(CrossData);
            return false;
        }

        var r = Radius;
        var _right = (Point02 - Point01).normalized;
        var a = (Point02 - Point01).magnitude / 2;
        var _up = new XVector3(_right.y + _right.z, -_right.x + _right.z, -_right.x - _right.y).normalized;
        var _forward = XVector3.Cross(_up, _right);
        var _midPoint = (Point02 + Point01) / 2;

        //var localPoint01 = new XVector3(a, XNumber.zero, XNumber.zero);
        //var localPoint02 = new XVector3(-a, XNumber.zero, XNumber.zero);

        var localSegPos01 = new XVector3(XVector3.Dot(segmentPos01 - _midPoint, _right), XVector3.Dot(segmentPos01 - _midPoint, _up), XVector3.Dot(segmentPos01 - _midPoint, _forward));
        var localSegPos02 = new XVector3(XVector3.Dot(segmentPos02 - _midPoint, _right), XVector3.Dot(segmentPos02 - _midPoint, _up), XVector3.Dot(segmentPos02 - _midPoint, _forward));

        var x1 = localSegPos01.x;
        var y1 = localSegPos01.y;
        var z1 = localSegPos01.z;

        var x2 = localSegPos02.x;
        var y2 = localSegPos02.y;
        var z2 = localSegPos02.z;

        var xMin = XIntMath.Min(localSegPos01.x, localSegPos02.x);
        var yMin = XIntMath.Min(localSegPos01.y, localSegPos02.y);
        var zMin = XIntMath.Min(localSegPos01.z, localSegPos02.z);

        var xMax = XIntMath.Max(localSegPos01.x, localSegPos02.x);
        var yMax = XIntMath.Max(localSegPos01.y, localSegPos02.y);
        var zMax = XIntMath.Max(localSegPos01.z, localSegPos02.z);

        var dis = XNumber.MaxValue;
        XVector3 localCrossPos = XVector3.zero;

        // 左半球
        handleLeft(xMin, a, r, xMax, yMin, yMax, zMin, zMax, x1, x2, y1, y2, z1, z2, localSegPos01, ref dis, ref localCrossPos);

        // 圆柱
        handleMid(xMin, a, xMax, yMin, r, yMax, zMin, zMax, x1, x2, y1, y2, z1, z2, localSegPos01, ref dis, ref localCrossPos);

        // 右半球
        handleRight(xMin, a, xMax, r, yMin, yMax, zMin, zMax, x1, x2, y1, y2, z1, z2, localSegPos01, ref dis, ref localCrossPos);

        var tmpDis = XVector3.Distance(localSegPos01, localSegPos02);
        if (dis > tmpDis)
        {
            crossData = default(CrossData);
            return false;
        }
        else
        {
            crossData.CrossLerp = dis / tmpDis;
            crossData.CrossPos = segmentPos01 + (segmentPos02 - segmentPos01) * crossData.CrossLerp;

            XNumber tmpLerp = XIntMath.InverseLerp(-a, a, localCrossPos.x);
            var tmpPoint = XVector3.Lerp(Point01, Point02, tmpLerp);
            crossData.CrossNormal = (crossData.CrossPos - tmpPoint).normalized;
            return true;
        }
    }

    public bool cross(XCapsule3D other)
    {
        other.Radius += this.Radius;
        CrossData ret;
        return other.cross(this.Point01, this.Point02, out ret);
    }

    public bool cross(XCapsule3D other, out CrossData ret)
    {
        other.Radius += this.Radius;
        return other.cross(this.Point01, this.Point02, out ret);
    }

    public void draw()
    {
        DebugExtension.DebugCapsule(Point01, Point02, Radius);
    }
}
